#include "carpark.h"
#include "C_code_Log.h"
#include "control.h"
#include "motor.h"
#include "photoelectric.h"
#include "rotate.h"
#include "Mydelay.h"
#include "collisionSwitch.h"
#include "servo.h"
#include "camera.h"
#include "voice.h"
#include "jq8400.h"
/**
* 函数介绍：直接停车
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：
* 作者    ：
*/
uint8_t carParkMethod_default() {
  LOG_INFO("carParkMethod_default");
  motorStop();
  return 1;
}
/**

* 函数介绍：右光电停车
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：找点完成后进入慢速巡线模式，等待右光电触发停车
* 作者    ：

*/
uint8_t parkMethod_Pes_L() {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Slow;
    flag = 1;
  }
  if(flag == 1 && PES_LEFT == 0){
    motorStop();
    flag = 0;
    return 1;
  }
  return 0;
}
/**

* 函数介绍：右光电停车
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：找点完成后进入慢速巡线模式，等待右光电触发停车
* 作者    ：

*/
uint8_t parkMethod_Pes_R() {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Slow;
    flag = 1;
  }
  if(flag == 1 && PES_RIGHT == 0){
    motorStop();
    flag = 0;
    return 1;
  }
  return 0;
}
/**

* 函数介绍：左光电延时停车
* 输入参数：时间值：单位ms
* 输出参数：0：未完成，1：停车完成
* 返回值  ：
* 其他    ：找点完成后进入慢速巡线模式，等待右光电触发再延时后停车
* 作者    ：

*/
uint8_t parkMethod_Pes_L_delay(uint16_t time) {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Platform;
    flag = 1;
  }
  if(flag == 1 && PES_LEFT == 0){
    flag = 2;
  }
  if(flag == 2 && myDelay((uint32_t)parkMethod_Pes_L_delay,time) == true){
    flag = 0;
    return 1;
  }
  return 0;
}

/**

* 函数介绍：延时停车
* 输入参数：时间值：单位ms
* 输出参数：0：未完成，1：停车完成
* 返回值  ：
* 其他    ：找点完成后进入慢速巡线模式，等待右光电触发再延时后停车
* 作者    ：

*/
uint8_t parkMethod_L_delay(uint16_t time) {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Platform;
    flag = 1;
  }
  if(flag == 1&&myDelay((uint32_t)parkMethod_L_delay,time)==true){
   flag = 0;
   return 1;
  }
  return 0;
}
/**

* 函数介绍：右光电延时停车
* 输入参数：时间值：单位ms
* 输出参数：0：未完成，1：停车完成
* 返回值  ：
* 其他    ：找点完成后进入慢速巡线模式，等待右光电触发停车
* 作者    ：

*/
uint8_t parkMethod_Pes_R_delay(uint16_t time) {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Platform;
    flag = 1;
  }
  if(flag == 1 && PES_RIGHT == 0){
    flag = 2;
  }
  if(flag == 2 && myDelay((uint32_t)parkMethod_Pes_R_delay,time) == true){
    flag = 0;
    return 1;
  }
  return 0;
}
/**

* 函数介绍：左光电倒退停车
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：找点完成后进入慢速巡线模式，等待左光电触发停车
* 作者    ：

*/
uint8_t parkMethod_Pes_L_Back() {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Back;
    flag = 1;
  }
  if(flag == 1 && PES_LEFT == 0){
    motorStop();
    flag = 0;
    return 1;
  }
  return 0;
}
/**

* 函数介绍：右光电倒退停车
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：找点完成后进入慢速巡线模式，等待右光电触发停车
* 作者    ：

*/
uint8_t parkMethod_Pes_R_Back() {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Back;
    flag = 1;
  }
  if(flag == 1 && PES_RIGHT == 0){
    motorStop();
    flag = 0;
    return 1;
  }
  return 0;
}
/**

* 函数介绍：倒车左光电延时停车
* 输入参数：时间值：单位ms
* 输出参数：0：未完成，1：停车完成
* 返回值  ：
* 其他    ：找点完成后进入慢速倒退模式，等待左光电触发再延时后停车
* 作者    ：

*/
uint8_t parkMethod_Pes_L_Back_delay(uint16_t time) {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Back;
    flag = 1;
  }
  if(flag == 1 && PES_LEFT == 0){
    flag = 2;
  }
  if(flag == 2 && myDelay((uint32_t)parkMethod_Pes_L_Back_delay,time) == true){
    flag = 0;
    return 1;
  }
  return 0;
}
/**

* 函数介绍：倒车右光电延时停车
* 输入参数：时间值：单位ms
* 输出参数：0：未完成，1：停车完成
* 返回值  ：
* 其他    ：找点完成后进入慢速倒退模式，等待右光电触发再延时后停车
* 作者    ：

*/
uint8_t parkMethod_Pes_R_Back_delay(uint16_t time) {
  static uint8_t flag = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Back;
    flag = 1;
  }
  if(flag == 1 && PES_RIGHT == 0){
    flag = 2;
  }
  if(flag == 2 && myDelay((uint32_t)parkMethod_Pes_R_Back_delay,time) == true){
    flag = 0;
    return 1;
  }
  return 0;
}
/**
 * @brief 返回平台节点号对应的平台号
 * 
 * @param nodeNum 
 * @return uint8_t 1~8,0表示不是平台节点
 */
static uint8_t getPlatformNumber(uint8_t nodeNum) {
  //平台节点号，依次为1号平台的节点号，2号平台的节点号...
  const uint8_t platformNode[] = {1,4,7,12,37,47,27,24};
  for (int i = 0; i < sizeof(platformNode); i++) {
    if (nodeNum == platformNode[i]) {
      return i+1;
    }
  }
  return 0;
}
/* 平台得分动作 */
/**

* 函数介绍：平台景点得分动作
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：平台景点：机器人到达2-8号平台后，推碰挡板，露出景点标示牌。随后
            机器人先后举起左、右手臂，接着旋转≥180°，同时播报，例如“到达2号平
            台”，得该平台景点积分。每次出发，平台型景点分数只记录一次。
* 作者    ：
*/
uint8_t Platform_score(uint8_t Platform_number){
  static uint8_t flag = 0;
  if(flag == 0){
    //举起左臂
    setServoAngleByIndex(SERVO_LEFT_ARM,SERVO_LEFT_ARM_UP);
    flag = 1;
  }
  if(flag == 1 && myDelay((uint32_t)Platform_score, 300) == true){
    //举起右臂
    setServoAngleByIndex(SERVO_RIGHT_ARM,SERVO_RIGHT_ARM_UP);
    //到达？号平台 语音
    switch (Platform_number)
    {
    case 2:
      playMusic(VOICE_Platform2);
      break;
    case 3:
      playMusic(VOICE_Platform3);
      break;
    case 4:
      playMusic(VOICE_Platform4);
      break;
    case 5:
      playMusic(VOICE_Platform5);
      break;
    case 6:
      playMusic(VOICE_Platform6);
      break;
    case 7:
      playMusic(VOICE_Platform7);
      break;
    case 8:
      playMusic(VOICE_Platform8);
      break;
    default:
      break;
    }
    flag = 2;
  }
  if(flag == 2){
    //旋转≥180°
    if(rotAnglePlatform(180,Left) == 1){
      //放手
      setServoAngleByIndex(SERVO_LEFT_ARM,SERVO_LEFT_ARM_INIT);
      setServoAngleByIndex(SERVO_RIGHT_ARM,SERVO_RIGHT_ARM_INIT);
      flag = 0;
      return 1;
    }
  }
  return 0;
}
/**

* 函数介绍：宝物得分动作
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：寻宝计分：寻宝比赛中，机器人登上平台后，须先做平台景点规定动作，
            获得该平台景点积分。若该平台有宝物，机器人同时举起左右手臂，播报“发现
            宝物”，旋转 360 度后放下手臂，得到“宝物”积分。
* 作者    ：
*/
uint8_t Treasure_score(){
  static uint8_t flag = 0;
  if(flag == 0){
    //举起左臂
    setServoAngleByIndex(SERVO_LEFT_ARM,SERVO_LEFT_ARM_UP);
    //举起右臂
    setServoAngleByIndex(SERVO_RIGHT_ARM,SERVO_RIGHT_ARM_UP);
    //发现宝物 语音
    playMusic(VOICE_FIND_TREASURE);
    flag = 1;
  }
  if(flag == 1){
    //旋转≥180°
    if(rotAnglePlatform(360,Left) == 1){
      //放手
      setServoAngleByIndex(SERVO_LEFT_ARM,SERVO_LEFT_ARM_INIT);
      setServoAngleByIndex(SERVO_RIGHT_ARM,SERVO_RIGHT_ARM_INIT);
      flag = 0;
      return 1;
    }
  }
  return 0;
}
/**

* 函数介绍：平台停车（通用模板）
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：完成平台任务（信息获取、得分动作等）
* 作者    ：
！！！待完善
*/
uint8_t parkMethod_Platform(){
#if (GAME_MODE ==FIND_TREASURE)//如果是寻宝模式
  static uint32_t target = 0;
  static uint32_t AdTarget = 0; 
  static bool (*save)(void) = NULL;
  static uint8_t Platform_level;//平台等级,把所有平台分成4个等级,取值1~4
  static uint8_t needRecognize = 0;  // 是否需要识别数字，0：不需要，1：需要
  extern uint8_t Treasure[9];
  const int FIND_TREASURE_Overtime = 5000;  // 数字识别超时时间
#endif
  const int BACK_TIME = 140;//碰撞开关触发以后，以PWM值为-1200时的后退时间
  static uint8_t Platform_number;//平台号
  const int collisionTime = 500;//碰撞开关触发后继续前进的时间
  static enum Flag{
    INIT,//初始化
    APPROACH,//靠近的过程，等待前光电触发
    SLOW,//前光电触发后的慢速，等待碰撞开关触发
    WAIT,
    BACK,//碰撞开关触发后的后退
#if (GAME_MODE ==FIND_TREASURE)//如果是寻宝模式
    PROCESSING,//处理宝物数据
    ACTION_TREASURE,//宝物得分动作
#endif
    ACTION,//平台得分动作
  }flag;
  if(flag == INIT){
    Platform_number = getPlatformNumber(getNextNode(&gl_controlCenter));
#if (GAME_MODE ==FIND_TREASURE)//如果是寻宝模式
    target = 0;
    save = NULL;
    //获取平台号和等级
    Platform_level = (Platform_number-1)/2+1;
    //判断是否需要识别数字
    if(Platform_level < 4 && Treasure[2*Platform_level+1] == 0 && Treasure[2*Platform_level + 2] == 0){
      needRecognize = 1;
    }
    else{
      needRecognize = 0;
    }
#endif
    //切换为盲走模式
    gl_controlCenter.findLineWay = NFL_GYRO;
    gl_controlCenter.findLineFlag = 0;
    flag = APPROACH;
  }
  //前光电触发，进入慢速
  if(flag == APPROACH && PES_FRONT == 0){
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(600, 600);
#if (GAME_MODE == FIND_TREASURE)  // 如果是寻宝模式
    // 抬起摄像头
    if (needRecognize == 1) {
      setServoAngleByIndex(SERVO_CAMERA, SERVO_CAMERA_UP);
    }
#endif
    //抬起身体
    flag = SLOW;
  }
  //碰撞开关触发，后退一段
  if(flag == SLOW && collision_seekFront() == true )
  {
      flag = WAIT;
  } 
  if(flag == WAIT && myDelay((uint32_t)parkMethod_Platform,collisionTime)==true){
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(-1500, -1500);
    setServoAngleByIndex(SERVO_BODY, SERVO_BODY_INIT);
#if (GAME_MODE == FIND_TREASURE)  // 如果是寻宝模式
    if (needRecognize == 1) {
      // 摄像头切换到数字识别
      camera_SendCommand(CAMERA_CMD_NUMBER);
      // 切换刷新方法
      if (gl_controlCenter.refresh != refresh_Time) {
        save = gl_controlCenter.refresh;
        // 把刷新方法改为20ms刷新一次
        setRefreshFunction(&gl_controlCenter, refresh_Time);
      }
    }
#endif
    flag = BACK;
  }
  if(flag == BACK && myDelay((uint32_t)parkMethod_Platform,BACK_TIME)==true){
    motorStop();
#if (GAME_MODE == FIND_TREASURE)  // 如果是寻宝模式
    if (needRecognize == 1) {
      // 设置数字识别定时
      target = getGlobalTime() + FIND_TREASURE_Overtime;
      AdTarget = getGlobalTime() +1000;
      flag = PROCESSING;
    }
    else {
      flag = ACTION;
    }
#else
    flag = ACTION;
#endif
  }
#if (GAME_MODE ==FIND_TREASURE)//如果是寻宝模式
  if(flag == PROCESSING){
  //获取数字信息
    if(USART_DataIsReceived(&usartCamera))
    {
      USART_DataResetReceivedFlag(&usartCamera);
      uint8_t Treasure_clues = 0;//宝物线索

      Treasure_clues = getRightNumber();
      //获取的线索 为 当前平台级 的 下级 的 线索
      if((Treasure_clues-1)/2+1 == Platform_level+1){
        Treasure[Treasure_clues] = 1;//储存宝物线索
        playMusic(VOICE_GET_CLUE);//成功获取线索,语音提示

        //摄像头切换到寻线
        camera_SendCommand(CAMERA_CMD_FIND_LINE);
        if(save!=NULL){
          setRefreshFunction(&gl_controlCenter,save);
        }
        flag = ACTION;
      }
    }
    //超过八秒钟没识别到直接退出
    if(getGlobalTime() > target){
      camera_SendCommand(CAMERA_CMD_FIND_LINE);
      if(save!=NULL){
        setRefreshFunction(&gl_controlCenter,save);
      }
      flag = ACTION;
    }
    if(getGlobalTime()>AdTarget){
        AdTarget +=1000;
        static int isBack = 0;
        isBack++;
        if(isBack %2==0){
          speedAdjustment(-800,-800);
          HAL_Delay(150);
          motorStop();
        }
        else{
          speedAdjustment(800,800);
          HAL_Delay(150);
          motorStop();
        }
      }
  }
#endif
  // 平台动作需补充
  if (flag == ACTION) {
    if (Platform_score(Platform_number) == 1) {
#if (GAME_MODE == FIND_TREASURE)  // 如果是寻宝模式
      if (Treasure[Platform_number] == 1) {
        flag = ACTION_TREASURE;
        return 0;
      } else {
        if (needRecognize == 1) {
          setServoAngleByIndex(SERVO_CAMERA, SERVO_CAMERA_INIT);
        }
      }
#endif     
      setServoAngleByIndex(SERVO_BODY, SERVO_BODY_BACK);
      flag = INIT;
      return 1;
    }
  }
#if (GAME_MODE == FIND_TREASURE)  // 如果是寻宝模式
  if (flag == ACTION_TREASURE) {
    if (Treasure_score() == 1) {
      if (needRecognize == 1) {
        setServoAngleByIndex(SERVO_CAMERA, SERVO_CAMERA_INIT);
      }
      setServoAngleByIndex(SERVO_BODY, SERVO_BODY_BACK);
      flag = INIT;
      return 1;
    }
  }
#endif
  return 0;
}
/**
 * @brief 一号平台停车
 * 
 * @return uint8_t 
 */
uint8_t parkMethod_PlatformOne() {
  static uint8_t flag = 0;
  static uint32_t target = 0;
  if(flag == 0){
    gl_controlCenter.findLineWay = NFL_Platform;
    setServoAngleByIndex(SERVO_BODY, SERVO_BODY_INIT);
    flag = 1;
  }
  if(flag == 1 && myDelay((uint32_t)parkMethod_PlatformOne,100) == true){//延时500ms
    gl_controlCenter.findLineWay = FL_No;//把电机控制权收回
    motorStop();
    flag = 2;
  }
  if(flag == 2 && rotAnglePlatform_ONE(180,Right) == 1){//旋转180度
    flag = 0;
    return 1;
  } 
  return 0;
}
/**
 * @brief 碰撞开关停车，用于直立小景点
 * 
 * @return uint8_t 
 */
uint8_t parkMethod_collide() {
  static uint8_t flag = 0;
  //前光电触发，进入慢速
  if(flag == 0){
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(1000, 1000);
    flag = 1;
  }
  if(flag == 1 && myDelay((uint32_t)(parkMethod_collide),500))
  {
    playMusic(VOICE_SCEBIC_SPOT);
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(-1200, -1200);
    flag = 2;
  }
  //碰撞开关触发，后退一段
  if(flag == 1 && collision_seekFront() == true){
    playMusic(VOICE_SCEBIC_SPOT);
    gl_controlCenter.findLineWay = FL_No;
    speedAdjustment(-1200, -1200);
    flag = 2;
  }
  if(flag == 2 && myDelay((uint32_t)parkMethod_collide,200)==true){
    motorStop();
    flag = 0;
    return 1;
  }
  return 0;
}
uint8_t parkMethod_delay(uint16_t time){
  static uint8_t flag;
  if(flag == 0){
    flag = 1;
    gl_controlCenter.findLineWay = NFL_Platform;
  }
  if(flag == 1 && myDelay((uint32_t)parkMethod_delay,time) == true){
    flag = 0;
    return 1;
  }
  return 0;
}
#include "color.h"
Color gl_doorColor[4];//全局红绿灯颜色，分别表示39，40，41，42号红绿灯的颜色
/**
 * @brief 红绿灯停车
 * 
 * @return uint8_t 
 * @note 停车识别红绿灯并根据红绿灯颜色来切换路线
 */
static uint8_t parkMethod_Door() {
  // 0：找到点，1：已经到达可以识别颜色的位置，2：舵机开始旋转
  // 3：舵机转到位，如果绿灯，直接返回1并回到状态0。4：如果不通过此红绿灯，要切换路线，切换之前后退完成
  // 5：开始转180度，转完就切路线
  static uint8_t flag = 0;
  static const uint8_t *runTable;  // 指向要切换的路线图的位置
#define DOOR_DELAY 0  // 红绿灯前找到点到可以开始扫描的延时时间
#define DOOR_SERVO_DELAY 0  // 舵机转到位的时间
  Color door = COLOR_UNKNOW;
  if (flag == 0) {
    gl_controlCenter.findLineWay = FL_slow;
    gl_controlCenter.findLineFlag = 0;
    flag = 1;
  }
  if (flag == 1 && myDelay((uint32_t)parkMethod_Door, DOOR_DELAY) == true) {
    gl_controlCenter.findLineWay = FL_No;  // 收回电机控制权，避免轮询时被巡线任务控制
    gl_controlCenter.findLineFlag = 0;
    motorStop();
    flag = 2;
  }
  if (flag == 2) {
    // 舵机旋转指令
    setServoAngleByIndex(SERVO_COLOR, SERVO_COLOR_SCAN);
    flag = 3;
  }
  if (flag == 3 && myDelay((uint32_t)parkMethod_Door, DOOR_SERVO_DELAY)) {
    // 当前的红绿灯节点号,如，9~39路段，此时算出来的nowNode就是39
    uint8_t nowNode = gl_controlCenter.runMethodTable[gl_controlCenter.nodeNum];
    // 发送颜色识别指令,开始扫描
    while (door == COLOR_UNKNOW) {
      door = colorRecognize();
    }
    if (door == COLOR_RED) {
      playMusic(VOICE_COLOR_RED);
    } else if (door == COLOR_GREEN) {
      playMusic(VOICE_COLOR_GREEN);
    } else if (door == COLOR_YELLOW) {
      playMusic(VOICE_COLOR_YELLOW);
    }

    // 记下当前红绿灯的值，给其他地方的规划可以用
    gl_doorColor[nowNode - 39] = door;
    LOG_INFO("第%d个红绿灯为%d", nowNode - 39, (int)door);

    //舵机归位
    setServoAngleByIndex(SERVO_COLOR, SERVO_COLOR_INIT);

    // 对结果做判断分析
    if (door == COLOR_GREEN) {
      flag = 0;
      return 1;  // 继续往后走，进入转弯任务（此时转弯任务为不转弯，所以会走后面的路线）
    } else if(door == COLOR_YELLOW && nowNode == 39){
      // 根据nowNode的值和door的值自行判断，以下是一个例子
      // 假设现在是39号点，且为红灯或者黄灯，就切为runMethodDoor[0]的路线
      // 指向要切换的路线图的位置
      gl_doorColor[1] = COLOR_GREEN;
      runTable = runMethodDoor[1];
			changeRunMethodTable(&gl_controlCenter, mapInfo, runTable);
       flag = 0; 
			 return 1; 	
      } else if (door == COLOR_RED && nowNode == 39){
        gl_doorColor[1] = COLOR_GREEN;
        runTable = runMethodDoor[nowNode - 39];
        flag = 5;
      }
      else
      {runTable = runMethodDoor[nowNode - 39];
      flag = 5;  // 后退一小段后，原地转180，然后再切路线
      }
   
  }
  if (flag == 5 ) {
    // 切换路线，不需要调头，后面的路线图用后退的形式
    changeRunMethodTable(&gl_controlCenter, mapInfo, runTable);
    flag = 0;
    return 1;
  }
  return 0;
}
/// @brief 停车任务回调函数
/// @param control 
/// @note 在完成停车任务后调用，将停车任务状态设置为完成，将旋转任务状态置为准备就绪
static void carParkCallback(ControlCenterTypeDef *control) {
  control->runState->carParkState = MY_EXIT;
  //把巡线任务状态置为退出，旋转时不需要巡线，2024/6/2，by Sevenfite
  control->runState->F_LineState = MY_EXIT;
  control->runState->RotateState = MY_READY;
}

/// @brief 停车任务处理函数
/// @param control 控制台
/// @note 在停车任务状态为准备就绪时调用，根据停车任务的类型执行不同的停车任务
void carParkHandle_Task(ControlCenterTypeDef *control) {
  int result = 0;
  // 如果停车任务状态为准备就绪
  if (control->runState->carParkState == MY_READY) {
    // 根据停车任务的类型执行不同的停车任务，sectionInfo是当前路段的信息的指针，指向地图
    switch (control->sectionInfo->carParkWay) {
      case PARK_Default:
        result = carParkMethod_default();
        break;
      case PARK_pesL:
        result = parkMethod_Pes_L();
        break;
      case PARK_pesR:
        result = parkMethod_Pes_R();
        break;
      case PARK_pesL_200:
        result = parkMethod_Pes_L_delay(200);
        break;
      case PARK_pesR_200:
        result = parkMethod_Pes_R_delay(200);
        break;
      case PARK_pesL_300:
        result = parkMethod_Pes_L_delay(300);
        break;
      case PARK_pesR_300:
        result = parkMethod_Pes_R_delay(300);
        break;
      case PARK_pesL_Back:
        result = parkMethod_Pes_L_Back();
        break;
      case PARK_pesR_Back:
        result = parkMethod_Pes_R_Back();
        break;
      case PARK_pesL_Back_200:
        result = parkMethod_Pes_L_Back_delay(200);
        break;
      case PARK_pesL_Back_300:
        result = parkMethod_Pes_L_Back_delay(300);
        break;
      case PARK_pesL_Back_500:
        result = parkMethod_Pes_L_Back_delay(500);
        break;
      case PARK_pesR_Back_200:
        result = parkMethod_Pes_R_Back_delay(200);
        break;
      case PARK_pesR_Back_300:
        result = parkMethod_Pes_R_Back_delay(300);
        break;
      case PARK_pesR_Back_500:
        result = parkMethod_Pes_R_Back_delay(500);
        break;
      case PARK_Platform:
        result = parkMethod_Platform();
        break;
      case PARK_Platform_ONE:
        result = parkMethod_PlatformOne();
        break;
      case PARK_collide:
        result = parkMethod_collide();
        break;
      case PARK_DELAY_300:
        result = parkMethod_delay(300);
        break;
      case PARK_DELAY_500:
        result = parkMethod_delay(500);
        break;
      case PARK_DELAY_800:
        result = parkMethod_delay(800);
        break;
      case PARK_DELAY_1000:
        result = parkMethod_delay(1000);
        break;
      case DOOR:
        if (1 == parkMethod_Door()) {
          // 如果停车任务状态为准备就绪,即没有切换路线，如果有切换路线，状态应该为挂起态
          if (control->runState->carParkState == MY_READY) {
            result = 1;
          }
        }
        break;
      default: result = 1;
        break;
    }
    // 如果停车任务处理完成，调用停车任务回调函数
    if (result == 1) {
      carParkCallback(control);
    }
  }
}